#pragma once
#include <vector>
#include <thread>
#include "PointSet.h"
#include "State.h"
#include "Edge.h"
#include "_gpu_computation.h"

class Raptor
{
public:
	Raptor();
	~Raptor();
	
	void motion_planning_start();
	void motion_planning_ending();

	std::vector<void*>* get_probed_elements()
	{
		if (A_is_ready == true)
			return &probe_object_A;
		else
			return &probe_object_B;
	}

	std::vector<std::string> msg_output;

protected:
	Coord_Control_Bounds bounds;
	std::vector<_gpu_ticket> tickets;
	std::vector<_gpu_result> results;
	unsigned int _gpu_jobs;

	unsigned int dim_state;
	double* q_max;
	double* q_min;

	unsigned int dim_control;
	float* c_max;
	float* c_min;

	unsigned int n_threads;
	std::vector<std::thread*> thread_pool;
	std::thread* thread_probe;
	void motion_planning_threading(unsigned int id, unsigned int n_iterations);
	
	PointSet* v_active;
	State* root;
	void motion_planning_init();

	bool propagate_cpu(double* q_start, double* ctrl, double duration, double*& q_new, double& cost_step);
	double duration_inc;

	void motion_planning_probing();
	std::vector<double*> coord_current;
	std::vector<void*> probe_object_A;
	std::vector<void*> probe_object_B;
	bool A_is_ready = true;
};

